#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     Lift,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     FrontLeft,     tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     FrontRight,    tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     BackLeft,      tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     BackRight,     tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

void initializeRobot()
{
	motor[FrontRight] = 0;
	motor[FrontLeft] = 0;
	motor[BackRight] = 0;
	motor[BackLeft] = 0;
		return;
}

int threshold = 20;

void displayDebugData(int value)
{
	nxtDisplayClearTextLine(2);
	nxtDisplayTextLine(2, "%d", value);
	while(nNxtButtonPressed != 3)
		NULL;
	nxtDisplayClearTextLine(2);
}
void Forward (int power, int duration)
{
  motor[FrontRight]= power;
  motor[FrontLeft]= power;
  motor[BackRight] = power;
  motor[BackLeft] = power;
  wait1Msec(duration);
}
void Backward (int power, int duration)
{
  motor[FrontRight]= -1*power;
  motor[FrontLeft]= -1*power;
  motor[BackRight] = -1*power;
  motor[BackLeft] = -1*power;
  wait1Msec(duration);
}

void turnL (int power, int duration)
{
	motor[FrontRight] = power;
	motor[FrontLeft] = -1*power;
	motor[BackRight] = power;
	motor[BackLeft] = -1*power;
	wait1Msec(duration);
}

void turnR (int power, int duration)
{
	motor[FrontRight] = -1*power;
	motor[FrontLeft] = power;
	motor[BackRight] = -1*power;
	motor[BackLeft] = power;
	wait1Msec(duration);
}


void allStop ()
{
	motor[FrontRight] = 0;
	motor[FrontLeft] = 0;
	motor[BackRight] = 0;
	motor[BackLeft] = 0;
}



task main()
{
	initializeRobot();

	waitForStart();     // wait for start of tele-op phase

	while (true)
	{
		getJoystickSettings(joystick);

		//FORWARD
		if(joystick.joy1_TopHat == 0)
		{
			Forward(80,200);
		}

		//BACKWARD
		else if (joystick.joy1_TopHat == 4)
		{
			Backward(80,200);
		}

		//TURN LEFT
		else if (joystick.joy1_TopHat == 6)
		{
			turnL (50,200);
		}

		//TURN RIGHT
		else if (joystick.joy1_TopHat == 2)
		{
			turnR (50,200);
		}

		//DEFAULT
		else
		{
			allStop ();
		}

		if(abs(joystick.joy1_y1) > threshold)
		{
			if(joystick.joy1_y1 > 0)
			{
				motor[Lift] = 100;
			}
			else if(joystick.joy1_y1 < 0)
			{
				motor[Lift] = -30;
			}
		}
		else
		{
			motor[Lift] = 3;
		}
	}
}
